function observ = mag_hfun(modelObject,observObject, state, N, U2)

[dim,nov] = size(state);

observ = zeros(observObject.dim,nov);

dimX  = modelObject.statedim;

for k=1:nov,
    observ(:,k) = hfun(state(1:dimX,k), N(:,k), U2);
end


function observ = hfun(state, N, U2)
observ = hx(state,U2)+N;


function yp = hx(x, U)
yp = zeros(3,1);
q0 = x(1);
q1 = x(2);
q2 = x(3);
q3 = x(4);


DEFAULT_IMU_MAG_INCL = -60.98;
DEFAULT_IMU_MAG_DECL = -13.82;
DEG_TO_RAD = pi/180;
v0m = [0,0,0];
mag = [0,0,0];

mag(1) = cos(DEFAULT_IMU_MAG_INCL * DEG_TO_RAD);
mag(2) = 0.0;
mag(3) = sin(DEFAULT_IMU_MAG_INCL * DEG_TO_RAD);

v0m(2) = (mag(1) * cos(DEFAULT_IMU_MAG_DECL * DEG_TO_RAD) - mag(2) * sin(DEFAULT_IMU_MAG_DECL * DEG_TO_RAD));
v0m(1) = -(mag(2) * cos(DEFAULT_IMU_MAG_DECL * DEG_TO_RAD) + mag(1) * sin(DEFAULT_IMU_MAG_DECL * DEG_TO_RAD));
v0m(3) = (mag(3));

uu = rotatebyrevquat(v0m,x(1:4));

yp(1) = -uu(2);
yp(2) = uu(3);
yp(3) = -uu(1);



function vr = rotatebyrevquat(v, q)


w = q(1);
x = -q(2);
y = -q(3);
z = -q(4);

vr(1) = w*w*v(1) + 2.0*y*w*v(3) - 2.0*z*w*v(2) + x*x*v(1) + 2.0*y*x*v(2) + 2.0*z*x*v(3) - z*z*v(1) - y*y*v(1);
vr(2) = 2.0*x*y*v(1) + y*y*v(2) + 2.0*z*y*v(3) + 2.0*w*z*v(1) - z*z*v(2) + w*w*v(2) - 2.0*x*w*v(3) - x*x*v(2);
vr(3) = 2.0*x*z*v(1) + 2.0*y*z*v(2) + z*z*v(3) - 2.0*w*y*v(1) - y*y*v(3) + 2.0*w*x*v(2) - x*x*v(3) + w*w*v(3);
